/*
 *  This is the SmartCar Standing Code;
 *  Copyright (C) 2011-2012 WenX, SEU, P.R.China <wenxiao1992@gmail.com>
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef WENX_solve_H
#define WENX_solve_H

#include "const.h"
#include "sci.h"

int A,B;

void solve(){
int send;
  TEST_SOLVE_CT++;
 
  Degree_Distance_MID=3300-4*128+4*(PORTA&0xF0);
  
  
  Pre_offset = This_offset;
  This_offset= TargetSPD-TSPD;
  //if(PORTA&0x02)
  MAXOFFSET=600*(1-(TSPD>0?(TSPD):(-TSPD))/TargetSPD);
  //else
  //  MAXOFFSET=300;
  if(This_offset>MAXOFFSET)
    This_offset=MAXOFFSET;
  else if(This_offset<-MAXOFFSET)
    This_offset=-MAXOFFSET;
  Pre_Control_Speed = Control_Speed;
  
  
  
  Control_Speed =
  (16)*(Degree_Distance-Degree_Distance_MID)+2*(Omi_SPD0-15155)+
  /* Pre_Control_Speed+*/25*(This_offset-Pre_offset)+16*This_offset;         
  
  if(Degree_Distance>6000||Degree_Distance<1300)
    Control_Speed=0;
  
  ChangeLED(5,0);
  ChangeLED(6,0);
  ChangeLED(7,0);
  
  if((PORTA&0x04)==0){
    DCOffset=0;
    DC_Control=0;
  }else{
    DCOffset=DC3-DC1;
    DC_Control=DCOffset*Offset_P+(TSPDA-TSPDB)*Offset_D;
  }
  
  
  if((Control_Speed!=0)){
      
    if(DC_Control>0)
      SPDACC=-(TSPD+(0.0*TSPD*DC_Control))*1.5;
    else
      SPDACC=-(TSPD-(0.0*TSPD*DC_Control))*1.5;
    Control_Speed+=SPDACC;
    
    if(Control_Speed>10000)
      Control_Speed=10000;
    if(Control_Speed<-10000)
      Control_Speed=-10000;
    /*if(DC_Control>3)
      DC_Control=3;
    if(DC_Control<-3)
      DC_Control=-3;     */
    setspdB(Control_Speed*0.95-TSPD*DC_Control);
    setspdA(Control_Speed*1.05+TSPD*DC_Control);
  }else{
    setspdA(0);  
    setspdB(0);

  }

  //////////////////////////////////////
  
  PTS|=0x10;
  //Start flag
  SciWrite(0x35);
  SciWrite(0xA4);
  
  send = (int)(TSPDA);
  SciWrite((send>>8)&0xFF);
  SciWrite(send&0xFF);
 
  send = (int)(TSPDB);
  SciWrite((send>>8)&0xFF);
  SciWrite(send&0xFF);
  
  SciWrite(((int)(DC_Control*10000)>>8)&0xFF);
  SciWrite((int)(DC_Control*10000)&0xFF);
  
  SciWrite(((int)DCOffset>>8)&0xFF);
  SciWrite((int)DCOffset&0xFF);
  
  //////////////////////////////////////
  
  
  lastdelta=TargetSPD-TSPDA;;

}

#endif